#include <algorithm>

#include "../include/Bone.h"
#include "../include/VecMat.h"

// for mesage box for testing
#include <wx/msgdlg.h>
//

//#include "../src/sqrt.c"

unsigned int Bone::lastId = 0;
unsigned int Bone::bonesCounter = 0;


Bone::Bone(Bone* parent, bool jointNumber, wxString name, double angleMin, double angleMax)
:id(lastId++)
{
    this->selectedJoint = Bone::NON;
    this->hoveredJoint = Bone::NON;
    this->spriteFileName = _("");
    this->spritePos = wxPoint(0,0);
    this->spriteAngle = .0;
//    this->deltaAngle = .0;
//    this->angleOrginal = .0;
    this->loops = .0;
    this->motion = true;
    this->angleMin = angleMin;
    this->angleMax = angleMax;
    this->parent = parent;
    this->parent->children.push_back(this);
    this->jointNumber = jointNumber;
    this->loopsRelative = .0; // set by bone manager together with orginal angle
    ++bonesCounter;

    if (name.IsEmpty())
        this->name << _("Bone_") << bonesCounter;
    else
        this->name = name;

    if ( !jointNumber )
        begin = parent->begin;
    else
        begin = parent->end;

    end = new wxRealPoint(*begin);
}


Bone::Bone(wxRealPoint beginJoint, wxRealPoint endJoint, wxString name, double angleMin, double angleMax )
:id(lastId++)
{
    this->selectedJoint = Bone::NON;
    this->hoveredJoint = Bone::NON;
    this->jointNumber = Bone::NON;
    this->spriteFileName = _("");
    this->spritePos = wxPoint(0,0);
//    this->angleOrginal = .0;
    this->motion = true;
    this->angleMin = angleMin;
    this->angleMax = angleMax;
    this->spriteChanged = NULL;
    this->loopsRelative = .0;

    this->begin = new wxRealPoint( beginJoint );
    this->end = new wxRealPoint( endJoint );
    this->loops = -angleAbsolute() /(2*M_PI);

    ++bonesCounter;

    if (name.IsEmpty())
        this->name << _("Bone_") << bonesCounter;
    else
        this->name = name;

}

Bone::~Bone()
{
    if (parent == NULL)
        delete begin;

    delete end;
    --bonesCounter;

    if (bonesCounter == 0) // all bones removed
        lastId = 0;
}

Bone* Bone::getParent() const
{
    return parent;
}

unsigned int Bone::getBoneId() const
{
    return id;
}


std::list<Bone*>& Bone::getChildren()
{
    return children;
}

BoneState Bone::getBoneState(void) const
{
//    wxString test;
//    test << _("bone id =") << this->id;
//    test << _(" loops to save =") << this->loops;
//    wxMessageBox(test);
    return BoneState (id, this->loops, motion);
}

void Bone::setBoneState(BoneState state)
{
    //double angle = this->angleOrginal - (state.loopNumber == 0 ? 0 : (state.loopNumber < 0 ? -1 : 1)) * 2*M_PI - state.deltaAngle;
    double angle = /*this->angleOrginal*/ - ( state.loops - (int)state.loops ) * 2*M_PI;

     wxString test;
    /*if(angle > 2*M_PI || angle < (-2*M_PI) ){
        test << _("id = ") << state.boneId << _(" angle ") << angle;
        wxMessageBox(test);
    }*/


//    test << _("id = ") << this->id;
//    test << _(" orginal = ") << this->angleOrginal << _( "old loop = ") << this->loopNumber << _(" old Deltaangle = ") << this->deltaAngle;
//    test << _(" new loop = ") << state.loopNumber << _(" newDangle = ") << state.deltaAngle;
//    test << _(" +??? = ") << (state.loopNumber == 0 ? 0 : (state.loopNumber < 0 ? -1 : 1));
    //wxMessageBox(test);


    /*this->angleLastAbsolute = angleAbsolute();
    double a = angle - angleLastAbsolute;

    end->x -= begin->x;
    end->y -= begin->y;
    double xnew = end->x * cos(a) - end->y * sin(a);
    double ynew = end->x * sin(a) + end->y * cos(a);
    end->x = xnew + begin->x;
    end->y = ynew + begin->y;

    // rotate the children
    std::list<Bone*>::iterator it;
    for( it=children.begin(); it!=children.end(); ++it )
    {
        (*it)->rotate(end, a);
    }
*/
    //this->angleLastAbsolute = angleAbsolute();
    setAngle(angle);
    this->loops = state.loops;
    this->motion = state.motion;

    /*wxString test;
    test << _("id =") << this->id << _(" loop =") << this->loopNumber;
    wxMessageBox(test);*/
}


double Bone::angleToParent(void)
{
    if( parent )
    {
        double parentLength = parent->length();
        wxRealPoint* parentEnd = jointNumber ? parent->begin : parent->end;

        double parentVector [2] = { parentEnd->x - begin->x, parentEnd->y - begin->y };
        double currentVector [2] = { end->x - begin->x, end->y - begin->y };

        double dotProduct = currentVector[0]*parentVector[0] + currentVector[1]*parentVector[1];

        return acos( dotProduct/(parentLength*length()) );
    }

    return 0;
}


double Bone::length(void)
{
    return  sqrt(pow( end->x - begin->x, 2) + pow( end->y - begin->y, 2));
}



double Bone::angleAbsolute(void)
{
    return atan2(end->y - begin->y, end->x - begin->x);
}

void Bone::addDeltaAngle(double angle)
{
    //deltaAngle += angle;

    loops += angle / (2*M_PI);

/*
    if(deltaAngle > 2*M_PI)
    {
        deltaAngle -= 2*M_PI;
        ++loopNumber;
    }
    else if (deltaAngle < -2*M_PI)
    {
        deltaAngle += 2*M_PI;
        --loopNumber;
    }
    */
}

void Bone::rotate(double angle, JOINT BEGINorEND)
{
    // jesli obracasz w okol poczatku, to obroc siebie i dzieci
    if(BEGINorEND == BEGIN)
    {
        rotate( begin, angle);

        double dAngle = this->angleLastAbsolute - angleAbsolute();
        if (dAngle < -M_PI)
            dAngle += 2*M_PI;
        else if (dAngle > M_PI)
            dAngle -= 2*M_PI;

        // Checking min, max ranges
        double lastLoopsRelative = loopsRelative;
        loopsRelative += dAngle / (2*M_PI);

        if (angleMin != 0 || angleMax !=0 )
        {
            if ( loopsRelative < angleMin || loopsRelative > angleMax ){

                loopsRelative = (loopsRelative < angleMin) ? angleMin : angleMax;

                dAngle -= (loopsRelative - lastLoopsRelative)*2*M_PI;
                rotate(begin, dAngle);
                dAngle = 0;
            }
        }

        wxString test2;
        test2 << _("dangle= ") <<  dAngle;
       //wxMessageBox(test2);

        wxString test;

        std::list< Bone*> bones;
        bones.push_back(this);
        std::list< Bone* >::iterator listIt, childenIt;
        for ( listIt = bones.begin(); listIt != bones.end(); ++listIt)
        {
            for ( childenIt = ( (*listIt)->getChildren() ).begin(); childenIt != ( (*listIt)->getChildren() ).end(); ++childenIt)
            {
                bones.push_back( (*childenIt) );
            }
            (*listIt)->addDeltaAngle(dAngle);
            //test << _("id= ") << (*listIt)->id << _(" dangle ") << (*listIt)->deltaAngle << _("\n");
        }
    }
    // OBRACANIE TYLKO DZIECI -- JESZCZE NIE OBSLUGUJE STANOW (deltaangle....)
    // w przeciwnym wypadku obroc tylko dzieci
    else
    {

        std::list<Bone*>::iterator it;
        for( it=children.begin(); it!=children.end(); ++it )
            (*it)->rotate( end, angle);
    }
}

void Bone::rotate(wxRealPoint* referencePoint, double angle)
{
    double a = angle;
    this->angleLastAbsolute = angleAbsolute();

    end->x -= referencePoint->x;
    end->y -= referencePoint->y;

    double xnew = end->x * cos(a) - end->y * sin(a);
    double ynew = end->x * sin(a) + end->y * cos(a);

    end->x = xnew + referencePoint->x;
    end->y = ynew + referencePoint->y;

    // rotate the children
    std::list<Bone*>::iterator it;
    for( it=children.begin(); it!=children.end(); ++it )
    {
        (*it)->rotate(referencePoint, angle);
    }
}


void Bone::moveRootBegin(wxRealPoint translation)
{
    begin->x += translation.x;
    begin->y += translation.y;
}

void Bone::setEnd(wxRealPoint endPoint)
{
    *end = endPoint;
    angleLastAbsolute = angleAbsolute();
    loops = -angleLastAbsolute / (2*M_PI);
    loopsOrginal = loops;
}

unsigned int Bone::removeChild(Bone* child)
{
    // removing child's successors
    unsigned int removed = child->removeChildren();

    delete child;

    // remove child from bone's children list
    std::remove(children.begin(), children.end(), child);

    return removed + 1;
}


unsigned int Bone::removeChildren()
{
    unsigned int removed = 0;

    std::list<Bone*>::iterator it;
    for( it=children.begin(); it!=children.end(); ++it )
    {
        removed += removeChild(*it);
    }

    return removed;
}

void Bone::resetStaticFields()
{
    Bone::lastId = 0;
    Bone::bonesCounter = 0;
}

/*void Bone::setAngleOrginal()
{
//    this->angleOrginal = angleAbsolute();
//    if (id != 0) // Root has not parent
//        this->loopsRelative = angleToParent() / (2*M_PI);
    this->loopsRelative = 0;//loops;//angleAbsolute() / (2*M_PI);
}*/

double Bone::getLoopsOrginal() const
{
    return loopsOrginal;
}

double Bone::getLoops() const
{
    return loops;
}

void Bone::setLoops(double loops)
{
    this->loops = loops;
}

double Bone::getLoopsRelative() const
{
    return loopsRelative;
}

void Bone::setLoopsRelative(double loopsRelative)
{
    this->loopsRelative = loopsRelative;
}

bool Bone::isMotion() const
{
    return motion;
}

void Bone::setMotion(bool motion)
{
    this->motion = motion;
}

void Bone::setAngle(double angle)
{
    double sAngle, dAngle;

    sAngle = angleAbsolute();
    dAngle = angle-sAngle;

    rotate(dAngle, BEGIN );
}


void Bone::setAngleMin(double angleMin)
{
    this->angleMin = angleMin;
}


void Bone::setAngleMax(double angleMax)
{
    this->angleMax = angleMax;
}


double Bone::getAngleMin(void) const
{
    return angleMin;
}


double Bone::getAngleMax(void) const
{
    return angleMax;
}


void Bone::setAngleMinMax(double _angleMin, double _angleMax)
{
    angleMin = _angleMin;
    angleMax = _angleMax;
}


void Bone::setName(wxString _name)
{
    name = _name;
}


wxString Bone::getName(void) const
{
    return name;
}

wxString Bone::getSpriteFileName() const
{
    return spriteFileName;
}

void Bone::setSpriteFileName(wxString fileName)
{
    this->spriteFileName = fileName;
}

void Bone::draw(wxDC* dc)
{
    drawSprite(dc);
    drawSketch(dc);
}

void Bone::drawSketch(wxDC* dc)
{
    dc->SetPen(*wxGREY_PEN);
    dc->DrawLine(begin->x, begin->y, end->x, end->y);
    dc->SetPen(*wxBLACK_PEN);


    if ( selectedJoint == Bone::BEGIN || hoveredJoint == Bone::BEGIN)
    {
        dc->SetBrush(*wxBLACK_BRUSH);
        dc->DrawRectangle(begin->x - 2, begin->y - 2, 5, 5);
        dc->SetBrush(*wxTRANSPARENT_BRUSH);
        dc->DrawRectangle(end->x - 2, end->y - 2, 5, 5);

    }
    if ( selectedJoint == Bone::END || hoveredJoint == Bone::END)
    {
        dc->SetBrush(*wxTRANSPARENT_BRUSH);
        dc->DrawRectangle(begin->x - 2, begin->y - 2, 5, 5);
        dc->SetBrush(*wxBLACK_BRUSH);
        dc->DrawRectangle(end->x - 2, end->y - 2, 5, 5);
        return;
    }

        dc->SetBrush(*wxTRANSPARENT_BRUSH);
        dc->DrawRectangle(begin->x - 2, begin->y - 2, 5, 5);
        dc->DrawRectangle(end->x - 2, end->y - 2, 5, 5);

}

void Bone::drawSprite(wxDC* dc)
{
    if ( spriteOriginal.IsOk() )
    {
        double a = angleAbsolute();
        double pi2 = atan(1)*2;
        int w = spriteOriginal.GetWidth();
        int h = spriteOriginal.GetHeight();
        double sx = spritePos.x;
        double sy = spritePos.y;

        //if ( fabs(a-angleLastAbsolute ) > 0.001 || spriteChanged )
        {
            spriteChanged = NULL;
            angleLastAbsolute = a;
            spritePrepared = spriteOriginal.Rotate( -a-pi2, wxPoint(0,0));
        }

            if( a>pi2 )
            {
                dc->DrawBitmap( wxBitmap( spritePrepared ), getBegin()->x-sin(a-pi2)*(h+sy)-cos(a-pi2)*(w+sx)/2.0, getBegin()->y-sin(a-pi2)*(w+sx)/2.0+cos(a-pi2)*(sy));
            }

            else if ( a>0 && a<=pi2 )
            {
                dc->DrawBitmap( wxBitmap( spritePrepared ), getBegin()->x-sin(a)*(w+sx)/2.0+cos(a)*(sy), getBegin()->y-cos(a)*(w-sx)/2.0+sin(a)*(sy));
            }

            else if( a>-pi2 && a<=0)
            {
                dc->DrawBitmap( wxBitmap( spritePrepared ), getBegin()->x-sin(-a)*(w-sx)/2.0+cos(-a)*(sy), getBegin()->y-sin(-a)*(h+sy)-cos(-a)*(w-sx)/2.0);
            }

            else if ( a<=-pi2 )
            {
                dc->DrawBitmap( wxBitmap( spritePrepared ), getBegin()->x-sin(-a-pi2)*(h+sy)-cos(-a-pi2)*(w-sx)/2.0, getBegin()->y-cos(-a-pi2)*(h+sy)-sin(-a-pi2)*(w+sx)/2.0);
            }
        //    dc->SetPen(*wxBLACK_PEN);
        //     bones
        //    if( a>pi2 )
        //        dc->DrawRectangle( getBegin()->x-sin(a-atan(1)*2)*h-cos(a-atan(1)*2)*w/2, getBegin()->y-sin(a-atan(1)*2)*w/2, spritePrepared.GetWidth(), spritePrepared.GetHeight() );
        //
        //    else if ( a>0 && a<=pi2 )
        //        dc->DrawRectangle( getBegin()->x-sin(a)*w/2.0, getBegin()->y-cos(a)*w/2.0, spritePrepared.GetWidth(), spritePrepared.GetHeight());
        //
        //    else if( a>-pi2 && a<=0)
        //        dc->DrawRectangle( getBegin()->x-sin(-a)*w/2.0, getBegin()->y-sin(-a)*h-cos(-a)*w/2.0, spritePrepared.GetWidth(), spritePrepared.GetHeight());
        //
        //    else if ( a<=-pi2 )
        //        dc->DrawRectangle( getBegin()->x-sin(-a-pi2)*h-cos(-a-pi2)*w/2.0, getBegin()->y  -cos(-a-pi2)*h-sin(-a-pi2)*w/2.0, spritePrepared.GetWidth(), spritePrepared.GetHeight() );
    }
}

void Bone::hoverJoint(JOINT BeginEndNon)
{
    hoveredJoint = BeginEndNon;
}

Bone::JOINT Bone::getHoveredJoint() const
{
    return hoveredJoint;
}

void Bone::selectJoint(JOINT BeginEndNon)
{
    selectedJoint = BeginEndNon;
}

int Bone::getSelectedJoint() const
{
    return selectedJoint;
}

bool Bone::getJointNumber() const
{
    return jointNumber;
}

wxRealPoint* Bone::getBegin() const
{
    return begin;
}

wxRealPoint* Bone::getEnd() const
{
    return end;
}

void Bone::setSprite( wxImage sprite )
{
    spriteChanged = TRUE;
    spriteOriginal = sprite;
    spritePrepared = sprite;
}

void Bone::setSpritePos( wxPoint pos )
{
    spritePos = pos;
}

wxPoint Bone::getSpritePos() const
{
    return spritePos;
}

void Bone::setSpriteAngle( double angle )
{
    spriteAngle = angle;
}
